#include "app_tentacle.h"
#include "app.h"
#include "bsp_timer.h"

// 触角控制测试程序

// 测试状态
typedef enum {
    TENTACLE_TEST_IDLE = 0,
    TENTACLE_TEST_POSITION,
    TENTACLE_TEST_CIRCLE,
    TENTACLE_TEST_SPIRAL,
    TENTACLE_TEST_STOP
} tentacle_test_state_t;

static tentacle_test_state_t g_test_state = TENTACLE_TEST_IDLE;
static uint32_t g_test_timer = 0;
static uint32_t g_test_counter = 0;

// 触角测试初始化
void Tentacle_Test_Init(void)
{
    // 初始化触角控制
    Tentacle_Init();
    
    // 使能触角控制
    Tentacle_Enable(1);
    
    g_test_state = TENTACLE_TEST_IDLE;
    g_test_timer = 0;
    g_test_counter = 0;
    
    DEBUG("Tentacle Test Initialized\n");
}

// 触角测试处理函数，每10ms调用一次
void Tentacle_Test_Handle(void)
{
    g_test_timer += 10;  // 10ms
    
    switch (g_test_state) {
        case TENTACLE_TEST_IDLE:
            // 等待测试开始
            break;
            
        case TENTACLE_TEST_POSITION:
            // 位置控制测试
            if (g_test_timer >= 5000) {  // 5秒后切换到下一个测试
                g_test_timer = 0;
                g_test_counter++;
                
                // 设置不同的目标位置
                switch (g_test_counter % 4) {
                    case 0:
                        Tentacle_Set_Target_Position(30, 0, 80);
                        DEBUG("Tentacle Test: Target (30, 0, 80)\n");
                        break;
                    case 1:
                        Tentacle_Set_Target_Position(0, 30, 80);
                        DEBUG("Tentacle Test: Target (0, 30, 80)\n");
                        break;
                    case 2:
                        Tentacle_Set_Target_Position(-30, 0, 80);
                        DEBUG("Tentacle Test: Target (-30, 0, 80)\n");
                        break;
                    case 3:
                        Tentacle_Set_Target_Position(0, -30, 80);
                        DEBUG("Tentacle Test: Target (0, -30, 80)\n");
                        break;
                }
            }
            break;
            
        case TENTACLE_TEST_CIRCLE:
            // 圆形运动测试
            if (g_test_timer >= 10000) {  // 10秒后停止
                g_test_timer = 0;
                Tentacle_Stop();
                g_test_state = TENTACLE_TEST_STOP;
                DEBUG("Tentacle Test: Circle motion completed\n");
            }
            break;
            
        case TENTACLE_TEST_SPIRAL:
            // 螺旋运动测试
            if (g_test_timer >= 15000) {  // 15秒后停止
                g_test_timer = 0;
                Tentacle_Stop();
                g_test_state = TENTACLE_TEST_STOP;
                DEBUG("Tentacle Test: Spiral motion completed\n");
            }
            break;
            
        case TENTACLE_TEST_STOP:
            // 测试完成
            break;
    }
}

// 开始位置控制测试
void Tentacle_Test_Start_Position(void)
{
    g_test_state = TENTACLE_TEST_POSITION;
    g_test_timer = 0;
    g_test_counter = 0;
    
    Tentacle_Set_Mode(TENTACLE_MODE_POSITION);
    Tentacle_Set_Target_Position(30, 0, 80);
    
    DEBUG("Tentacle Test: Position control started\n");
}

// 开始圆形运动测试
void Tentacle_Test_Start_Circle(void)
{
    g_test_state = TENTACLE_TEST_CIRCLE;
    g_test_timer = 0;
    
    Tentacle_Set_Mode(TENTACLE_MODE_CIRCLE);
    
    DEBUG("Tentacle Test: Circle motion started\n");
}

// 开始螺旋运动测试
void Tentacle_Test_Start_Spiral(void)
{
    g_test_state = TENTACLE_TEST_SPIRAL;
    g_test_timer = 0;
    
    Tentacle_Set_Mode(TENTACLE_MODE_SPIRAL);
    
    DEBUG("Tentacle Test: Spiral motion started\n");
}

// 停止测试
void Tentacle_Test_Stop(void)
{
    g_test_state = TENTACLE_TEST_STOP;
    Tentacle_Stop();
    
    DEBUG("Tentacle Test: Stopped\n");
}

// 获取测试状态
tentacle_test_state_t Tentacle_Test_Get_State(void)
{
    return g_test_state;
}

// 打印触角状态信息
void Tentacle_Test_Print_Status(void)
{
    DEBUG("Tentacle Status:\n");
    DEBUG("  Current: (%.1f, %.1f, %.1f)\n", 
          g_tentacle_ctrl.current.x, 
          g_tentacle_ctrl.current.y, 
          g_tentacle_ctrl.current.z);
    DEBUG("  Target:  (%.1f, %.1f, %.1f)\n", 
          g_tentacle_ctrl.target.x, 
          g_tentacle_ctrl.target.y, 
          g_tentacle_ctrl.target.z);
    DEBUG("  Lines:   %.1f, %.1f, %.1f\n", 
          g_tentacle_ctrl.current.line1_length,
          g_tentacle_ctrl.current.line2_length,
          g_tentacle_ctrl.current.line3_length);
    DEBUG("  Mode:    %d\n", g_tentacle_ctrl.mode);
    DEBUG("  Enable:  %d\n", g_tentacle_ctrl.enable);
}

// 触角控制命令处理
void Tentacle_Test_Process_Command(uint8_t cmd, float param1, float param2, float param3)
{
    switch (cmd) {
        case 0x01:  // 设置目标位置
            Tentacle_Set_Target_Position(param1, param2, param3);
            DEBUG("Tentacle: Set target position (%.1f, %.1f, %.1f)\n", param1, param2, param3);
            break;
            
        case 0x02:  // 开始位置控制测试
            Tentacle_Test_Start_Position();
            break;
            
        case 0x03:  // 开始圆形运动测试
            Tentacle_Test_Start_Circle();
            break;
            
        case 0x04:  // 开始螺旋运动测试
            Tentacle_Test_Start_Spiral();
            break;
            
        case 0x05:  // 停止测试
            Tentacle_Test_Stop();
            break;
            
        case 0x06:  // 打印状态
            Tentacle_Test_Print_Status();
            break;
            
        case 0x07:  // 设置最大速度
            g_tentacle_ctrl.max_speed = param1;
            DEBUG("Tentacle: Set max speed %.1f mm/s\n", param1);
            break;
            
        case 0x08:  // 设置圆形半径
            g_tentacle_ctrl.circle_radius = param1;
            DEBUG("Tentacle: Set circle radius %.1f mm\n", param1);
            break;
            
        default:
            DEBUG("Tentacle: Unknown command 0x%02X\n", cmd);
            break;
    }
}
